#ifndef _TRACKER_H_
#define _TRACKER_H_

#include "opencv2/opencv.hpp"

#include <vector>
#include <utility>

using namespace std;

#include "DATObject.h"

#define DEFAULT_TRACKER_PRIOR_CONFIDENCE 1.0

#define TRACK_IN_FRAME_MEDIAN 1
#define TRACK_IN_FRAME_HOMOGRAPHY 2

#define TRACK_IN_FRAME_DEFAULT TRACK_IN_FRAME_HOMOGRAPHY

#define DEFAULT_MANEUVER 1.1


class Tracker
{
public:
	Tracker() : mKF(8, 4, 0)
	{
		mLastSeenDATObject = NULL;
		mFlag = TRACK_IN_FRAME_DEFAULT;
		mPriorConfidence = DEFAULT_TRACKER_PRIOR_CONFIDENCE;
		mLastWasTracked = 0;
		mID = mIDCount;
		mIDCount++;

		mDebugPredict = 0;
		mDebugCorrect = 0;
		mDebugChDelta = 0;

		mManeuver = DEFAULT_MANEUVER;
		initKalman(0,0,0,0);
	};
	Tracker(DATObject* inDATObject) : mKF(8, 4, 0)
	{
		mLastSeenDATObject = inDATObject;
		mPriorConfidence = DEFAULT_TRACKER_PRIOR_CONFIDENCE;
		mFlag = TRACK_IN_FRAME_DEFAULT;
		mLastWasTracked = 0;
		mID = mIDCount;
		mIDCount++;

		mDebugPredict = 0;
		mDebugCorrect = 0;
		mDebugChDelta = 0;

		mManeuver = DEFAULT_MANEUVER;
		initKalman(inDATObject->mLastPosition.x,inDATObject->mLastPosition.y,inDATObject->mLastPosition.width,inDATObject->mLastPosition.height);
	};
	Tracker(DATObject* inDATObject, double inPriorConfidence) : mKF(8, 4, 0)
	{
		mLastSeenDATObject = inDATObject;
		mPriorConfidence = inPriorConfidence;
		mFlag = TRACK_IN_FRAME_DEFAULT;
		mLastWasTracked = 0;
		mID = mIDCount;
		mIDCount++;

		mDebugPredict = 0;
		mDebugCorrect = 0;
		mDebugChDelta = 0;

		mManeuver = DEFAULT_MANEUVER;
		initKalman(inDATObject->mLastPosition.x,inDATObject->mLastPosition.y,inDATObject->mLastPosition.width,inDATObject->mLastPosition.height);
	};
	virtual ~Tracker()
	{
		if(mLastSeenDATObject) 
			delete mLastSeenDATObject;
	};

	//The confidence about the answer (if its "true" then, its the confidence of the "true" answer... 
	//otherwise, its the confidence of the false answer... the false confidence only have sense for "compareObjects")
	virtual bool compareObjects(DATObject* inDATObject, double* outConfidence) = 0;
	virtual bool findObjectInFrame(DATFrame* inDATFrame, std::pair<cv::Point, double>& outRectTransformFound, double* outConfidence) = 0;	//in the negative case, it could return more than one value.. .so, perhaps outConfidence should be a pointer
	virtual bool findObjectInFrame(DATFrame* inDATFrame, cv::Rect inROI, std::pair<cv::Point, double>& outRectTransformFound, double* outConfidence)
	{
		if(inROI.width <= 0 || inROI.height <= 0)
		{
			*outConfidence = 0.0;
			return false;
		}
		cv::Point lPtROI; cv::Size lSizeROI;
		inDATFrame->mActualFrame.locateROI(lSizeROI,lPtROI);
		int lLeft = inROI.x; int lTop = inROI.y;
		int lRight = lSizeROI.width - lLeft - inROI.width;  
		lRight = lRight < 0 ? 0 : lRight;
		int lDown = lSizeROI.height - lTop - inROI.height;  
		lDown = lDown < 0 ? 0 : lDown;
		//cv::imshow("Test1",cv::Mat(inDATFrame->mActualFrame,inROI));
		inDATFrame->mActualFrame.adjustROI(-lTop,-lDown,-lLeft,-lRight);
		//cv::imshow("Test2",inDATFrame->mActualFrame);
		//cv::waitKey();
		//cv::destroyWindow("Test2");
		inDATFrame->mROI = inROI;
		bool lOk = findObjectInFrame(inDATFrame,outRectTransformFound,outConfidence);
		inDATFrame->mActualFrame.adjustROI(lTop,lDown,lLeft,lRight);
		inDATFrame->mROI = cv::Rect(0,0,0,0);
		//cv::imshow("Test3",inDATFrame->mActualFrame);
		return lOk;
	};	

	//these function WILL clone the image
	virtual void update(DATObject* inDATObject, double inAliveConfidenceThreshold = ALIVE_CONFIDANCE, int inLifeCounterThreshold = ALIVE) = 0;
	virtual void update(std::pair<cv::Point,double>& inRectTransformFound, cv::Mat& inFrame, unsigned int inFramePos) = 0;	//The frame is needed for the IplImage in the DATObject tracked



	//get/set
	DATObject* getLastSeenDATObject()const{return mLastSeenDATObject;}
	//const int getHalfLife()const{return mHalfLife;};
	//const int getHalfDeath()const{return mHalfDeath;};
	const double getPriorConfidence()const{return mPriorConfidence;}
	const int getFlag()const{return mFlag;}
	const int getLastWasTracked()const{return mLastWasTracked;}
	const LivingState getLivingState()const{return mLastSeenDATObject->mLivingState;}

	virtual  void setLastSeenDATObject(DATObject* inLastSeenDATObject/*, bool inDelOptionOFF = false*/)
	{
		if(mLastSeenDATObject /*&& !inDelOptionOFF*/)
			delete mLastSeenDATObject;
		mLastSeenDATObject = inLastSeenDATObject;
	}
	//void setHalfLife(int inHalfLife){mHalfLife = inHalfLife;};
	//void setHalfDeath(int inHalfDeath){mHalfDeath = inHalfDeath;};
	void setPriorConfidence(double inPriorConfidence){mPriorConfidence=inPriorConfidence;}
	void setFlag(int inFlag){mFlag=inFlag;}
	void setLivingState(LivingState inLivingState){mLastSeenDATObject->mLivingState=inLivingState;}

	void deathIsComing(){if(mLastSeenDATObject->mDeathCounter > 0) mLastSeenDATObject->mDeathCounter = mLastSeenDATObject->mDeathCounter -1;};

	const unsigned int getID() const {return mID;};
	void setID(unsigned int inID){mID = inID;}; //DON'T USE EXCEPT WHEN MERGING!!!!

	void initKalman(int x, int y, int w, int h)
	{
		mKF.statePre.at<float>(0) = (float)x;
		mKF.statePre.at<float>(1) = (float)y;
		mKF.statePre.at<float>(2) = (float)w;
		mKF.statePre.at<float>(3) = (float)h;
		mKF.statePre.at<float>(4) = 0.0;
		mKF.statePre.at<float>(5) = 0.0;
		mKF.statePre.at<float>(6) = 0.0;
		mKF.statePre.at<float>(7) = 0.0;
		
		mKF.statePost.at<float>(0) = (float)x;
		mKF.statePost.at<float>(1) = (float)y;
		mKF.statePost.at<float>(2) = (float)w;
		mKF.statePost.at<float>(3) = (float)h;
		mKF.statePost.at<float>(4) = 0.0;
		mKF.statePost.at<float>(5) = 0.0;
		mKF.statePost.at<float>(6) = 0.0;
		mKF.statePost.at<float>(7) = 0.0;

		mKF.transitionMatrix = cv::Mat::eye(8,8,CV_32F); //*(Mat_<float>(4, 4) << 1,0,0,0,   0,1,0,0,  0,0,1,0,  0,0,0,1); //4x4 or 8x8 ?
		changeKalmanDelta(1);

		cv::setIdentity(mKF.measurementMatrix);
		cv::setIdentity(mKF.processNoiseCov, cv::Scalar::all(1e-4));
		cv::setIdentity(mKF.measurementNoiseCov, cv::Scalar::all(1e-1));
		cv::setIdentity(mKF.errorCovPost, cv::Scalar::all(.1));

		//correctKalman(cv::Rect(x,y,w,h));
		//cout << mKF.predict() << endl;
	}
	void changeKalmanDelta(int delta)
	{
		mKF.transitionMatrix.at<float>(0,4) = (float)delta;	//add dx, dy, dw and dh to x,y,dy,dw... this value should change every time delta change
		//mKF.transitionMatrix.at<float>(0,6) = -0.5*(float)delta;
		mKF.transitionMatrix.at<float>(1,5) = (float)delta;
		//mKF.transitionMatrix.at<float>(1,7) = -0.5*(float)delta;
		mKF.transitionMatrix.at<float>(2,6) = (float)delta;
		mKF.transitionMatrix.at<float>(3,7) = (float)delta;
	}

	//Clear the former and restart the other with the history
	virtual void updateKalman()
	{
		if(mLastSeenDATObject->mHistory.mPositions.size()>0)
			initKalman(mLastSeenDATObject->mHistory.mPositions[0].x,mLastSeenDATObject->mHistory.mPositions[0].y,mLastSeenDATObject->mHistory.mPositions[0].width,mLastSeenDATObject->mHistory.mPositions[0].height);
		for(unsigned int i=1;i<mLastSeenDATObject->mHistory.mPositions.size();i++)
		{
			int lDelta = mLastSeenDATObject->mHistory.mFrameMapping[i]-mLastSeenDATObject->mHistory.mFrameMapping[i-1];
			predictionKalman(lDelta);
			correctKalman(mLastSeenDATObject->mHistory.mPositions[i]);
		}
	}
	virtual cv::Rect correctKalman(cv::Rect inMeasurement)
	{
		cv::Mat lMatMeasurement = (cv::Mat_<float>(4,1) << (float)inMeasurement.x, (float)inMeasurement.y, (float)inMeasurement.width, (float)inMeasurement.height);

		cv::Mat lEstimated = mKF.correct(lMatMeasurement);
		cv::Rect lRectEstimated((int)lEstimated.at<float>(0),(int)lEstimated.at<float>(1),(int)lEstimated.at<float>(2),(int)lEstimated.at<float>(3));

		return lRectEstimated;
	}
	virtual cv::Rect predictionKalman(int delta)
	{
		changeKalmanDelta(delta);
		
		cv::Mat lPrediction = mKF.predict();
		cv::Rect lPositionPrediction((int)lPrediction.at<float>(0),(int)lPrediction.at<float>(1),(int)lPrediction.at<float>(2),(int)lPrediction.at<float>(3));
		if(mLastSeenDATObject->mHistory.mFrameMapping.size()==0)
		{
			//pffff, ok, lets try that small little change...
			lPositionPrediction = mLastSeenDATObject->mLastPosition;
		}

		return lPositionPrediction;
	}
	virtual cv::Rect expandPrediction(cv::Rect inPrediction, int delta)
	{
		int lXManeuver = ceil((inPrediction.width*(pow(mManeuver,(double)delta)-1)));
		int lYManeuver = ceil((inPrediction.height*(pow(mManeuver,(double)delta)-1)));
		inPrediction.x = inPrediction.x - lXManeuver/2;
		inPrediction.y = inPrediction.y - lYManeuver/2;
		inPrediction.width = inPrediction.width + lXManeuver/2;
		inPrediction.height = inPrediction.height + lYManeuver/2;
		return inPrediction;
	}
protected:
	DATObject*			mLastSeenDATObject;	//
	//int				mHalfLife;			//The max number of time, in a row ?, that this object was seen
	//int				mHalfDeath;			//The actual number of time, in a row, that this object wasn't seen since it's apparition

	//It's the confidence I accord to the tracker (VS the confidence that we can accord to a specific value given by the tracker)
	double				mPriorConfidence;	//]0 .. 1] value. alpha in my litterature.

	int					mFlag;

	int					mLastWasTracked;	//0 = no, 1 = yes //COULD BE  A BOOL RIGHT NOW

	//ID
	unsigned int		mID;
	static unsigned int mIDCount;

	//Kalman filter
	cv::KalmanFilter	mKF;
	double				mManeuver;
	int					mDebugCorrect;
	int					mDebugPredict;
	int					mDebugChDelta;
};

#endif
